#include <board.h>
#include <pit/pitd.h>
#include <cp15/cp15.h>
#include <clock.h>

////////////////////////////////////////////////////////////////////////////////
// CACHE

#if defined (AT91C_IRAM_1)
#define SRAM_ADDRESS  AT91C_IRAM_1
#define SRAM_SIZE     AT91C_IRAM_1_SIZE
#elif defined(AT91C_IRAM)
#define SRAM_ADDRESS  AT91C_IRAM
#define SRAM_SIZE     AT91C_IRAM_SIZE
#elif defined(AT91C_ISRAM)
#define SRAM_ADDRESS  AT91C_ISRAM
#define SRAM_SIZE     AT91C_ISRAM_SIZE
#else
#error SRAM define
#endif

#define SDRAM_ADDRESS 0x20000000

unsigned int pTranslationTable[4*1024] __attribute__((section(".trans")));

static void InitMMU()
{
    int i;

    extern int _snocache;
    extern int _enocache;

    // Program the TTB
    _writeTTB((unsigned int) pTranslationTable);

    // Program the domain access register
    _writeDomain(0xC0000000); // domain 15: access are not checked

    // Program level 1 page table entry
    // Vector adress
    for (i = 0; i < 0xB; ++i)
        pTranslationTable[i] = (i << 20) | // Physical Address
                (1 << 10) | // Access in supervisor mode
                (15 << 5) | // Domain
                (1 << 4) | (0 << 3) | // No D cache
                0x2; // Set as 1 Mbyte section

    // Reset table entries
    for (; i < (SRAM_ADDRESS >> 20); ++i)
        pTranslationTable[i] = 0;

    // SRAM adress (with D cache)
    pTranslationTable[SRAM_ADDRESS >> 20] = (SRAM_ADDRESS) | // Physical Address
                    (1 << 10) | // Access in supervisor mode
                    (15 << 5) | // Domain
                    (1 << 4) | (1 << 3) | // D cache
                    0x2; // Set as 1 Mbyte section

    // Reset table entries
    for (++i; i < (SDRAM_ADDRESS >> 20); ++i)
        pTranslationTable[i] = 0;

    // SDRAM adress (with D cache)
    for (; i < (((int)&_snocache) >> 20); ++i)
        pTranslationTable[i] = (i << 20) | // Physical Address
                (1 << 10) | // Access in supervisor mode
                (15 << 5) | // Domain
                (1 << 4) | (1 << 3) | // D cache
                0x2; // Set as 1 Mbyte section

    // SDRAM adress (without D cache)
    for (; i < (((int)&_enocache) >> 20); ++i)
        pTranslationTable[i] = (i << 20) | // Physical Address
                (1 << 10) | // Access in supervisor mode
                (15 << 5) | // Domain
                (1 << 4) | (0 << 3) | // No D cache
                0x2; // Set as 1 Mbyte section

    // SDRAM adress (with D cache)
    for (; i < (SDRAM_ADDRESS >> 20) + 64; ++i)
        pTranslationTable[i] = (i << 20) | // Physical Address
                (1 << 10) | // Access in supervisor mode
                (15 << 5) | // Domain
                (1 << 4) | (1 << 3) | // D cache
                0x2; // Set as 1 Mbyte section

    // Reset table entries
    for (; i < 0xFFF; ++i)
        pTranslationTable[i] = 0;

    // Peripherals adress
    pTranslationTable[0xFFF] = (0xFFF00000) | // Physical Address
            (1 << 10) | // Access in supervisor mode
            (15 << 5) | // Domain
            (1 << 4) | (0 << 3) | // No D cache
            0x2; // Set as 1 Mbyte section
}

////////////////////////////////////////////////////////////////////////////////
// TEST

static unsigned char rgb [BOARD_LCD_WIDTH * BOARD_LCD_HEIGHT * 3];

// QVGA
#define _FRAME_WIDTH    320
#define _FRAME_HEIGHT   240
/*/// QCIF
#define _FRAME_WIDTH    176
#define _FRAME_HEIGHT   144
//*/

static unsigned char y [_FRAME_WIDTH * _FRAME_HEIGHT];
static unsigned char u [(_FRAME_WIDTH * _FRAME_HEIGHT) >> 2];
static unsigned char v [(_FRAME_WIDTH * _FRAME_HEIGHT) >> 2];

static void yuv2rgb_with_rotation(unsigned char* image,
                                  const unsigned char* py,
                                  const unsigned char* pu,
                                  const unsigned char* pv,
                                  const int width,
                                  const int height,
                                  const int y_stride,
                                  const int uv_stride);

#define INTERRUPT_PERIOD 1e3 // microseconds
#define N_TESTS 500

volatile static unsigned long long elapsed;
static void timer(void)
{
    ++elapsed;
}

int main(void)
{
    unsigned int i;

    CLOCK_SetConfig(1);

    InitMMU();
    CP15_EnableMMU();
    CP15_Enable_D_Cache();
    CP15_Enable_I_Cache();

    PITD_Configure(timer, INTERRUPT_PERIOD, 1);
    PITD_Start();

    /* Lengthy operation */
    for (i = 0; i < N_TESTS; ++i) {
        yuv2rgb_with_rotation(rgb,
                y, u, v,
                _FRAME_WIDTH, _FRAME_HEIGHT,
                _FRAME_WIDTH, _FRAME_WIDTH >> 1);
    }

    PITD_Stop();

    CP15_Disable_I_Cache();
    CP15_Disable_D_Cache();
    CP15_DisableMMU();

    return 0;
}

/*
 * The code bellow is an adaptation of a code found at C. Matthew Curtin's
 * page form the Department of Computer Science and Engineering at The Ohio
 * State University.
 *
 * Only the function that converts Y'UV 4:2:0 to RGB 24b was kept.
 *
 * The original code did not rotate the image nor blacked the unused screen,
 * both applications are specific for the program using this conversion.
 *
 * The following block comment was untouched from the original .c file.
 */
/*
 * yuv2rgb.c, Software YUV to RGB coverter
 *
 *  Copyright (C) 1999, Aaron Holtzman <aholtzma@ess.engr.uvic.ca>
 *  All Rights Reserved.
 *
 *  Functions broken out from display_x11.c and several new modes
 *  added by H�kan Hjort <d95hjort@dtek.chalmers.se>
 *
 *  15 & 16 bpp support by Franck Sicard <Franck.Sicard@solsoft.fr>
 *
 *  This file is part of mpeg2dec, a free MPEG-2 video decoder
 *
 *  mpeg2dec is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *
 *  mpeg2dec is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

/* matrix coefficients */
#define CRV     117504
#define CBU     138453
#define CGU     13954
#define CGV     34903

static unsigned char clip(int y, int common)
{
    int i = (y + common) >> 16;
    return (unsigned char) (i < 0 ? 0 : (i > 255 ? 255 : i));
}

static void yuv2rgb_with_rotation(unsigned char* image,
                                  const unsigned char* py,
                                  const unsigned char* pu,
                                  const unsigned char* pv,
                                  const int width,
                                  const int height,
                                  const int y_stride,
                                  const int uv_stride)
{
    int y, u, v;
    int r_common, g_common, b_common;
    int i, j;

    int posX = 0, posY;
    unsigned char *odd, *even;

    const unsigned char* py_line_1;
    const unsigned char* py_line_2;

    py_line_1 = py;
    py_line_2 = py + y_stride;

    for (j = 0, posY = -2; j < height / 2; j++) {
        posY += 2;
        for (i = 0, posX = BOARD_LCD_WIDTH; i < width / 2; i++) {
            posX -= 2;

            //Common to all four pixels
            u = (*pu++) - 128;
            v = (*pv++) - 128;

            r_common = CRV * v + 32768;
            g_common = CGU * u + CGV * v - 32768;
            b_common = CBU * u + 32768;

            even = image + 3 * (posX * BOARD_LCD_HEIGHT + posY);
            odd = even + 3 * BOARD_LCD_HEIGHT;

            //Pixel I
            y = 76309 * ((*py_line_1++) - 16);
            *odd++ = clip(y, r_common);
            *odd++ = clip(y, -g_common);
            *odd++ = clip(y, b_common);

            //Pixel II
            y = 76309 * ((*py_line_1++) - 16);
            *even++ = clip(y, r_common);
            *even++ = clip(y, -g_common);
            *even++ = clip(y, b_common);

            //Pixel III
            y = 76309 * ((*py_line_2++) - 16);
            *odd++ = clip(y, r_common);
            *odd++ = clip(y, -g_common);
            *odd = clip(y, b_common);

            //Pixel IV
            y = 76309 * ((*py_line_2++) - 16);
            *even++ = clip(y, r_common);
            *even++ = clip(y, -g_common);
            *even = clip(y, b_common);
        }

        py_line_1 += y_stride;
        py_line_2 += y_stride;
        pu += uv_stride - width / 2;
        pv += uv_stride - width / 2;
    }

    // Black those out-of-bounds pixels:
    if (posY < BOARD_LCD_HEIGHT) { // bottom
        for (i = 0; i < BOARD_LCD_WIDTH; ++i) {
            odd = image + 3 * (i * BOARD_LCD_HEIGHT + posY);
            for (j = posY; j < BOARD_LCD_HEIGHT; ++j) {
                *odd++ = 0;
                *odd++ = 0;
                *odd++ = 0;
            }
        }
    }
    if (posX) { // right
        for (i = 0; i < posX; ++i) {
            odd = image + 3 * i * BOARD_LCD_HEIGHT;
            for (j = 0; j < posY; ++j) {
                *odd++ = 0;
                *odd++ = 0;
                *odd++ = 0;
            }
        }
    }
}
